#ifndef APPLY_RIGID_TRANSFORM_H
#define APPLY_RIGID_TRANSFORM_H

#include <cuda_runtime.h>


class ApplyRigidTransform{
public:

	struct applyTransform{
		float R[9];
		float t[3];
		bool n;

		__host__ applyTransform(float* rotation, float* translation, bool negative = false){
			for(int i = 0; i < 9; i++){
				R[i] = rotation[i];
			}
			for(int i = 0; i < 3; i++){
				t[i] = translation[i];
			}			
			n = negative;
		}

		__host__ __device__ float3 operator()(float3 in_out){
			if(abs(in_out.z) > 1.0){
				float temp[3];

				if(n){
					in_out.x *= -1.0;
					in_out.y *=  1.0;
					in_out.z *= -1.0;
				}

				for(int i = 0; i < 3; i++){
					temp[i] = 
						in_out.x * R[i * 3] + 
						in_out.y * R[i * 3 + 1] + 
						in_out.z * R[i * 3 + 2] + 
						t[i];
				}

				in_out.x =  temp[0];
				in_out.y =  temp[1];
				in_out.z =  temp[2];
			}
			return in_out;
		}




	};

	
	ApplyRigidTransform(){};
	~ApplyRigidTransform(){};
	void applyRT(float3* in_out, float* R, float* t, int size);
	void applyIntrinsicInv(float3* in_out, float* A, int size);


private:


};

#endif